package com.hc.hc_bot_kit

import android.annotation.TargetApi
import android.content.Context
import android.os.Build
import android.text.TextUtils
import com.blankj.utilcode.util.GsonUtils
import com.google.gson.Gson
import com.robint.RosController
import com.robint.ros.bean.RosLocationBean
import com.robint.ros.bean.RosMapBean
import com.robint.ros.data.RosTaskType
import com.robint.ros.utils.RosLocationUtils
import com.robint.task.handler.INavTaskHandler
import com.robint.task.param.NavTaskParam
import com.robint.utils.RosThreadPoolManager
import okhttp3.Callback
import java.util.concurrent.CompletableFuture
import java.util.concurrent.Future
import java.util.function.Consumer

open class RosMain {
    private var isInit = false

    fun init(ip: String, context: Context, result: (Boolean) -> Unit) {
        RobotManager.getInstance().initSdk(ip, context)
        RobotManager.getInstance()
            .checkRosReady { aBoolean ->
                isInit = aBoolean ?: false
                result(isInit)
                //清除底盘当前任务，防止有任务时APP重启，底盘任务还在，APP下次下发任务执行冲突
                //Clear the current task of the chassis. Prevent the APP from restarting when the task is executed, and the chassis task is still there.
                //Because the task still exists after restarting the APP, so the next time the task is sent, it will cause an execution conflict
                RosController.getInstance().clearTask(null)
            }
    }

    fun setOnNavigationListener(listener: NavigationLisenter?) {
        RobotManager.getInstance().setOnNavigationLisenter(listener)
    }

    fun navigationTo(dst: String): Boolean {
        val mapList = RosLocationUtils.getRosMapList()
        var currentMapUuid = RosController.getInstance().getCurrentMappuid()
        if (mapList.isEmpty() || currentMapUuid.isEmpty()) {
            return false
        }

        val mapBean = mapList.find { rosMapBean ->
            rosMapBean.getMap_uuid() == currentMapUuid
        } ?: return false

        val location = mapBean.loc_list.find { bean ->
            bean.location_name.removeSuffix("\n") == dst
        } ?: return false

        excuteNavTask(
            location
        )
        return true
    }

    fun excuteNavTask(next: RosLocationBean): INavTaskHandler {

        val currentMappuid = RosLocationUtils.getCurrentMappuid()
        //通过RosController创建一个到目标点的NavTask
        //Create a NavTask to the target point through RosController
        var arg2 = ""
        var arg3 = RosTaskType.CLEANING
        if (TextUtils.isEmpty(next.getLocation_uuid()) == true) {
            arg2 = next.cleaning_area_uuid
            arg3 = RosTaskType.CLEANING
        } else {
            arg2 = next.getLocation_uuid()
            arg3 = RosTaskType.NAVIGATION
        }

        val navTask = RosController.getInstance()
            .createNavTask(
                NavTaskParam(
                    currentMappuid,
                    arg2,
                    arg3
                )
            )

        RosThreadPoolManager.getInstance()
            .execute { //同步执行导航到目标点，此方法为阻塞方法，除非到达目标点或者去目标点的过程中发生异常，否则方法会一直阻塞
                //Synchronously execute navigation to the target point, this method is a blocking method, unless an exception occurs during the process of reaching the target point or going to the target point, the method will always be blocked
                navTask.executeSync()
                //导航到达了目标点,或者执行任务发生了异常并且主动调用了navTask.cancel()方法--->
            }
        return navTask
    }


}